/*
 * @文件描述:传感器管理,负责和下位机通过串口通讯，并获取传感器数据
 * @版本:
 * @作者:周晨阳
 * @Date:2021-10-14 16:39:36
 */
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Threading;
using LibUsbDotNet.DeviceNotify;
using MathNet.Filtering;
using MathNet.Filtering.FIR;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using System.Text;
using static Motor_Control.COM_CODE;

namespace Motor_Control
{
    /// <summary>
    /// 传感器连接状态
    /// </summary>
    public enum SENSOR_CONNECTION
    {
        OK = 1,
        DISCONNECT = 0,
        CONNECTING = 2,

    }
    /// <summary>
    /// 传感器ID
    /// </summary>
    public enum SENSOR_ID
    {
        SENSOR_1,
        SENSOR_2,
        SENSOR_3,
        SENSOR_4,
        SENSOR_5,
        SENSOR_6,
        SENSOR_7,
        SENSOR_ALL,
    }
    /// <summary>
    /// 卡尔曼滤波类，一个传感器用一个卡尔曼滤波对象，其中需要自行调整的参数有Q和R
    /// </summary>
    public class KalmanFilter
    {
        private double ADC_OLD_Value = 0;
        private double P_k1_k1 = 0;
        // Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；
        // 但是太小则容易发散，如果Q为零，那么我们只相信预测值；
        // Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；
        // 如果Q值无穷大，那么我们只信任测量值；

        // R值为测量噪声，太小太大都不一定合适。
        // R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；
        // 越小系统收敛越快，但过小则容易出现震荡；
        // 测试时可以保持陀螺仪不动，记录一段时间内陀螺仪的输出数据，
        // 这个数据近似正态分布，按3σ原则，取正态分布的(3σ)^2作为R的初始化值。

        //测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。
        public double Q = 0.1f;//Q值为过程噪声，越小系统越容易收敛
        public double R = 13.32275304f;//R值为测量噪声，太小太大都不一定合适。和ADC本身噪声分布有关
        private double Kg = 0;
        private double P_k_k1 = 1;

        private double kalman_adc_old = 0;

        /// <summary>
        /// 一个卡尔曼滤波的实现，输入一个实时数值，返回处理后的数据，该方法为实时计算
        /// </summary>
        /// <param name="value">要处理的数值</param>
        /// <returns></returns>
        public double kalman_filter(double value)
        {
            double x_k1_k1, x_k_k1;
            double Z_k;
            double kalman_adc;

            Z_k = value;
            // if (Math.Abs(kalman_adc_old - value) >= 10)
            // {
            //     x_k1_k1 = value * 0.382f + kalman_adc_old * 0.618f;
            // }
            // else
            {
                x_k1_k1 = kalman_adc_old;
            }
            x_k_k1 = x_k1_k1;
            P_k_k1 = P_k1_k1 + Q;

            Kg = P_k_k1 / (P_k_k1 + R);

            kalman_adc = x_k_k1 + Kg * (Z_k - kalman_adc_old);
            P_k1_k1 = (1 - Kg) * P_k_k1;
            P_k_k1 = P_k1_k1;

            ADC_OLD_Value = value;
            kalman_adc_old = kalman_adc;

            return kalman_adc;
        }
    }
    /// <summary>
    /// 传感器类，该类为单例模式，用于连接、管理角度传感器，并对数据进行滤波处理等
    /// </summary>
    public class Sensors
    {
        //定义一个静态变量来保存类的实例
        static Sensors _Singleton;
        private static object Singleton_Lock = new object();
        //传感器数据数组
        private double[] sensorValueList = new double[8];
        private double[] sensorRawList = new double[8];
        private SerialPort serialPort;
        private MotorControlView view;
        private SENSOR_CONNECTION state = SENSOR_CONNECTION.DISCONNECT;
        private Thread readThread;
        private IDeviceNotifier UsbDeviceNotifier = DeviceNotifier.OpenDeviceNotifier();
        private OnlineFilter[] filterList = new OnlineFilter[7];
        private KalmanFilter[] kFilterList = new KalmanFilter[7];


        private Sensors()
        {
            serialPort = new SerialPort();
            for (int i = 0; i < filterList.Length; i++)
            {
                filterList[i] = OnlineFirFilter.CreateLowpass(MathNet.Filtering.ImpulseResponse.Finite, 40, 2, 3);
                kFilterList[i] = new KalmanFilter();
            }
            //注册接收回调函数
            InvokeCommand sendSensorValue = new InvokeCommand(REQUEST_CODE.SEND, TYPE_CODE.SENSOR_ANGLE, this.socketCallback);
            kFilterList[(int)SENSOR_ID.SENSOR_1].R = 13.62706393f;
            kFilterList[(int)SENSOR_ID.SENSOR_2].R = 9.093124034f;
            kFilterList[(int)SENSOR_ID.SENSOR_3].R = 12.70280981f;
            kFilterList[(int)SENSOR_ID.SENSOR_4].R = 13.39895938f;
            kFilterList[(int)SENSOR_ID.SENSOR_5].R = 12.97799877f;
            kFilterList[(int)SENSOR_ID.SENSOR_6].R = 13.32275304f;

        }
        /// <summary>
        /// 获得sensor对象
        /// </summary>
        /// <returns></returns>
        public static Sensors CreateInstance()
        {
            if (_Singleton == null) //双if +lock
            {
                lock (Singleton_Lock)
                {
                    if (_Singleton == null)
                    {
                        _Singleton = new Sensors();
                    }
                }
            }
            return _Singleton;
        }
        public Sensors getInstance()
        {
            return this;
        }
        /// <summary>
        /// 绑定view对象
        /// </summary>
        /// <param name="view"></param>
        public void bandView(MotorControlView view)
        {
            this.view = view;
            UsbDeviceNotifier.OnDeviceNotify += OnDeviceNotifyEvent;
            view.onViewSetCOMPorts(getCOMList());
            getCOMList();
            view.onViewSetSerialState(SENSOR_CONNECTION.DISCONNECT);
            state = SENSOR_CONNECTION.DISCONNECT;
            for (int i = 0; i < 7; i++)
            {
                view.onViewSetSensorR((SENSOR_ID)(i), kFilterList[i].Q);

            }
        }
        /// <summary>
        /// 处理来自对方的数据请求，不需要显式调用，由Analysis类自动处理并调用
        /// </summary>
        /// <param name="sensorId"></param>
        private void socketCallback(object sensorId)
        {
            StringBuilder stringBuilder = new StringBuilder();
            //需要发送的sensorID
            int value0 = int.Parse(sensorId.ToString());
            //Console.WriteLine("ID:"+value0);
            double sendvalue0 = Sensors.CreateInstance().getValue((SENSOR_ID)value0);
            //如果SENSOR_ID为all的话，就需要一次性把所有传感器值都传过去，一共7个传感器数据
            if ((SENSOR_ID)value0 == SENSOR_ID.SENSOR_ALL)
            {
                for (int i = 1; i < 8; i++)
                {
                    stringBuilder = Analysis.formatString(REQUEST_CODE.SEND, TYPE_CODE.SENSOR_ANGLE, 2);
                    stringBuilder.Append("Value[0]=");
                    stringBuilder.Append(i - 1);
                    stringBuilder.Append(";");
                    stringBuilder.Append("Value[1]=");
                    stringBuilder.Append(Sensors.CreateInstance().getValue((SENSOR_ID)(i - 1)));
                    stringBuilder.Append(";");
                    MySocket.Send(MySocket.getWorkingSocket(), stringBuilder.ToString());
                    //Thread.Sleep(2);
                }
            }
            //否则就按要求单独传输传感器数据
            else
            {
                stringBuilder = Analysis.formatString(REQUEST_CODE.SEND, TYPE_CODE.SENSOR_ANGLE, 2);
                stringBuilder.Append("Value[0]=");
                stringBuilder.Append(value0);
                stringBuilder.Append(";");
                stringBuilder.Append("Value[1]=");
                stringBuilder.Append(sendvalue0.ToString());
                stringBuilder.Append(";");
                MySocket.Send(MySocket.getWorkingSocket(), stringBuilder.ToString());

            }

        }
        /// <summary>
        /// 获取传感器值
        /// </summary>
        /// <param name="id">角度传感器的ID</param>
        /// <returns>角度值或力度</returns>
        public double getValue(SENSOR_ID id)
        {
            return sensorValueList[(int)id];
        }
        /// <summary>
        /// 获取传感器未滤波的值
        /// </summary>
        /// <param name="id"></param>
        /// <returns></returns>
        public double getRawValue(SENSOR_ID id)
        {
            return sensorRawList[(int)id];
        }
        /// <summary>
        /// 获取已经连接的COM口
        /// </summary>
        /// <returns>COM口string[] </returns>
        public string[] getCOMList()
        {
            string[] ports = SerialPort.GetPortNames();

            Console.WriteLine("The following serial ports were found:");

            // Display each port name to the console.
            foreach (string port in ports)
            {
                Console.WriteLine(port);
            }
            return ports;
        }
        /// <summary>
        /// 设置卡尔曼的R值
        /// </summary>
        /// <param name="id"></param>
        /// <param name="r"></param>
        public void setSensorR(SENSOR_ID id, double r)
        {
            kFilterList[(int)id].R = r;
        }
        public void setSensorQ(SENSOR_ID id, double Q)
        {
            kFilterList[(int)id].Q = Q;
        }

        /// <summary>
        /// 连接到传感器
        /// </summary>
        /// <param name="com">com口</param>
        public void connectSensor(string com)
        {
            view.onViewSetSerialState(SENSOR_CONNECTION.CONNECTING);
            state = SENSOR_CONNECTION.CONNECTING;

            serialPort.PortName = com;
            serialPort.BaudRate = 115200;  //波特率，默认9600
            serialPort.DataBits = 8;  //数据位，默认8
            serialPort.StopBits = System.IO.Ports.StopBits.One;  //停止位，默认 System.IO.Ports.StopBits.One
            serialPort.Parity = System.IO.Ports.Parity.None;  //奇偶校验，默认 System.IO.Ports.Parity.None
            //serialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler);
            try
            {
                serialPort.Open();
                view.onViewSetSerialState(SENSOR_CONNECTION.OK);
                state = SENSOR_CONNECTION.OK;

                readThread = new Thread(receiveData);

                readThread.Start();


            }
            catch (System.IO.IOException e)
            {
                Console.WriteLine(e.ToString());
                state = SENSOR_CONNECTION.DISCONNECT;
                view.onViewSetSerialState(state);
                view.onViewDisplayError(e);

            }
        }
        /// <summary>
        /// 断开与传感器的连接
        /// </summary>
        public void disconnectSensor()
        {
            try
            {
                serialPort.Close();


            }
            catch (System.IO.IOException e)
            {
                Console.WriteLine(e.ToString());
                view.onViewDisplayError(e);

            }
            finally
            {
                state = SENSOR_CONNECTION.DISCONNECT;
                view.onViewSetSerialState(SENSOR_CONNECTION.DISCONNECT);

            }
        }
        /// <summary>
        /// 获取传感器的连接状态
        /// </summary>
        /// <returns></returns>
        public SENSOR_CONNECTION getSensorConnection()
        {
            return state;
        }
        /// <summary>
        /// 当由新的USB插入或者拔出时,会调用此函数,用于更新com口列表
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void OnDeviceNotifyEvent(object sender, DeviceNotifyEventArgs e)
        {
            //事件发生  

            Console.WriteLine(e.ToString());//将事件输出  
            view.onViewSetCOMPorts(getCOMList());
        }

        /// <summary>
        /// 读取数据线程，该线程会根据下位机发送的数据更新传感器数据数组并更新ui数据
        /// </summary>
        private void receiveData()
        {

            while (state == SENSOR_CONNECTION.OK && serialPort.IsOpen)
            {
                try
                {
                    string s = serialPort.ReadLine();
                    //view.onViewShowDebugInfo(s);
                    analysisString(s);

                }
                catch (System.Exception e)
                {
                    //serialPort.Close();
                    Console.WriteLine(e);
                    state = SENSOR_CONNECTION.DISCONNECT;
                    view.onViewSetSerialState(state);
                }
                view.onViewSetAngle(SENSOR_ID.SENSOR_1, getValue(SENSOR_ID.SENSOR_1));
                view.onViewSetAngle(SENSOR_ID.SENSOR_2, getValue(SENSOR_ID.SENSOR_2));
                view.onViewSetAngle(SENSOR_ID.SENSOR_3, getValue(SENSOR_ID.SENSOR_3));
                view.onViewSetAngle(SENSOR_ID.SENSOR_4, getValue(SENSOR_ID.SENSOR_4));
                view.onViewSetAngle(SENSOR_ID.SENSOR_5, getValue(SENSOR_ID.SENSOR_5));
                view.onViewSetAngle(SENSOR_ID.SENSOR_6, getValue(SENSOR_ID.SENSOR_6));
                view.onViewSetAngle(SENSOR_ID.SENSOR_7, getValue(SENSOR_ID.SENSOR_7));

                view.onViewPlot(SENSOR_ID.SENSOR_1, getRawValue(SENSOR_ID.SENSOR_6));
                view.onViewPlot(SENSOR_ID.SENSOR_2, getValue(SENSOR_ID.SENSOR_6));

                //Console.WriteLine(getRawValue(SENSOR_ID.SENSOR_1));
                // view.onViewPlot(SENSOR_ID.SENSOR_2, getValue(SENSOR_ID.SENSOR_2));
                // view.onViewPlot(SENSOR_ID.SENSOR_3, getValue(SENSOR_ID.SENSOR_3));
                // view.onViewPlot(SENSOR_ID.SENSOR_4, getValue(SENSOR_ID.SENSOR_4));
                // view.onViewPlot(SENSOR_ID.SENSOR_5, getValue(SENSOR_ID.SENSOR_5));
                // view.onViewPlot(SENSOR_ID.SENSOR_6, getValue(SENSOR_ID.SENSOR_6));
                // view.onViewPlot(SENSOR_ID.SENSOR_7, getValue(SENSOR_ID.SENSOR_7));


            }

        }

        /// <summary>
        /// 分析下位机发送过来的字符串并给相应的变量赋值
        /// </summary>
        /// <param name="s"></param>
        private void analysisString(string s)
        {
            try
            {
                string[] args = s.Split(new char[2] { '=', ';' });
                string whichSensor = args[0];
                string num = whichSensor[1].ToString();
                double value = double.Parse(args[1]);
                int sensorNum = int.Parse(num);
                //Console.WriteLine("sensorNum:" + sensorNum + "value:" + value);
                //Console.WriteLine("sensorNum:" + sensorNum + "value:" + value);
                //Console.WriteLine( value);

                sensorRawList[sensorNum - 1] = value;
                //sensorValueList[sensorNum - 1] = filterList[sensorNum - 1].ProcessSample(value);
                sensorValueList[sensorNum - 1] = kFilterList[sensorNum - 1].kalman_filter(value);

            }
            catch (System.Exception)
            {

            }


        }
    }
}